/*
******************************************************************************************************* 
**  Copyright (C) 2019, 苏州检易生物科技有限公司 
**  All rights reserved. 
** 
**  FileName:       	pump.c
**  Description:	
**  Author:        	 
**  Version				v0.1
**  Date:           	2019-11-20
**	Function List： 
**  History:         
*********************************************************************************************************
*/


/*
*********************************************************************************************************
*                                              	调试开关
*********************************************************************************************************
*/


/*
*********************************************************************************************************
*                                             INCLUDE FILES
*********************************************************************************************************
*/
#include "stdl_claw.h"
#include "log_console.h"
#include "timer.h"
#include "system_cfg.h"
#include "can_callback.h"
#include "can_task.h"


/*
*********************************************************************************************************
*                                              	宏定义
*********************************************************************************************************
*/
/*
*********************************************************************************************************
*                                              	结构体定义
*********************************************************************************************************
*/

 
/*
*********************************************************************************************************
*                                              	函数原型声明
*********************************************************************************************************
*/
static stdl_claw_status_t stdl_claw_poll(claw_index_t index);
static void claw_task_tmr_callback(void* p_arg);
static void can_claw_tx_callback(uint16_t can_id,uint8_t dlc,uint8_t* p_data,can_bus_err_t err);
static void can_claw_rx_callback(uint16_t can_id,uint8_t dlc,uint8_t* p_data,can_bus_err_t err);
/*
*********************************************************************************************************
*                                              	全局变量定义
*********************************************************************************************************
*/
/*
*********************************************************************************************************
*                                              	模块静态变量定义
*********************************************************************************************************
*/
static stdl_claw_t m_claw[STDL_CLAW_NUM] = 
{
	{.node_id = 0x21,.status = STDL_CLAW_UNINIT}
};

static rt_timer_t						p_claw_timer;
/*
*********************************************************************************************************
*                                              	函数定义
*********************************************************************************************************
*/
void stdl_claw_start(void)
{
	p_claw_timer = rt_timer_create("claw timer",claw_task_tmr_callback,RT_NULL,CLAW_TMR_PERIOD,RT_TIMER_FLAG_PERIODIC);
	if(p_claw_timer == NULL)
	{
		Critical_Log("%s create timer failed!\r\n","claw timer");
	}
	else
		rt_timer_start(p_claw_timer);
	for(uint8_t index = 0;index < STDL_CLAW_NUM;index ++)
	{
		can_pct_regist(CAN_BUS_MOTOR,m_claw[index].node_id,can_claw_tx_callback,can_claw_rx_callback);
	}
	
}
stdl_claw_status_t stdl_claw_init(claw_index_t index)
{
	ASSERT(index < STDL_CLAW_NUM);
	can_mem_node_t* p_node = can_mem_malloc();
	if(p_node == NULL)
		return STDL_CLAW_UNINIT;
	claw_cmd_init(m_claw[index].node_id,&p_node->can_msg);
	app_msg_send(CAN_MSG_PUSH,(uint32_t)p_node,&g_can_task.mailbox);
	rt_base_t it = rt_hw_interrupt_disable();
	m_claw[index].status = STDL_CLAW_INITING;
	rt_hw_interrupt_enable(it);
	return STDL_CLAW_INITING;
}
stdl_claw_status_t stdl_claw_open(claw_index_t index)
{
	ASSERT(index < STDL_CLAW_NUM);
	can_mem_node_t* p_node = can_mem_malloc();
	if(p_node == NULL)
		return STDL_CLAW_OPENNING;
	claw_cmd_open(m_claw[index].node_id,&p_node->can_msg);
	app_msg_send(CAN_MSG_PUSH,(uint32_t)p_node,&g_can_task.mailbox);
	rt_base_t it = rt_hw_interrupt_disable();
	m_claw[index].status = STDL_CLAW_OPENNING;
	rt_hw_interrupt_enable(it);
	return STDL_CLAW_OPENNING;
}
stdl_claw_status_t stdl_claw_close(claw_index_t index)
{
	ASSERT(index < STDL_CLAW_NUM);
	can_mem_node_t* p_node = can_mem_malloc();
	if(p_node == NULL)
		return STDL_CLAW_CLOSING;
	claw_cmd_close(m_claw[index].node_id,&p_node->can_msg);
	app_msg_send(CAN_MSG_PUSH,(uint32_t)p_node,&g_can_task.mailbox);
	
	rt_base_t it = rt_hw_interrupt_disable();
	m_claw[index].status = STDL_CLAW_CLOSING;
	rt_hw_interrupt_enable(it);
	return STDL_CLAW_CLOSING;
}
static void claw_task_tmr_callback(void* p_arg)
{
	if(can_get_motor_list_size() >	CAN_SEND_LIST_MAX/2)
		return;
	for(uint8_t index = 0;index < STDL_CLAW_NUM;index ++)
	{
		if(m_claw[index].status == STDL_CLAW_INITING || 
			m_claw[index].status == STDL_CLAW_OPENNING || 
		m_claw[index].status == STDL_CLAW_CLOSING)
		{
			if(m_claw[index].polling_flag == 0)
			{
				m_claw[index].polling_flag = 1;
				stdl_claw_poll(index);
			}
		}
	}
}
static stdl_claw_status_t stdl_claw_poll(claw_index_t index)
{
	ASSERT(index < STDL_CLAW_NUM);
	can_mem_node_t* p_node = can_mem_malloc();
	if(p_node == NULL)
		return STDL_CLAW_CLOSING;
	claw_cmd_poll(m_claw[index].node_id,&p_node->can_msg);
	app_msg_send(CAN_MSG_PUSH,(uint32_t)p_node,&g_can_task.mailbox);
	return 0;
}
static void can_claw_tx_callback(uint16_t can_id,uint8_t dlc,uint8_t* p_data,can_bus_err_t err)
{
	return;
}
static void can_claw_rx_callback(uint16_t can_id,uint8_t dlc,uint8_t* p_data,can_bus_err_t err)
{
	rt_base_t it = rt_hw_interrupt_disable();
	claw_data_t* p_recv = (claw_data_t*)p_data;
	if(CAN_ID_GET_FN(can_id) == FN_SDO_R)
	{
		if(p_recv->sdo_cs == SDO_CS_READ_ACK && p_recv->regaddr == CLAW_REG_STATUS)
		{
			for(uint8_t index = 0;index < STDL_CLAW_NUM;index ++)
			{
				if(m_claw[index].node_id == CAN_ID_GET_NODE(can_id))
				{
					if(m_claw[index].polling_flag)
						m_claw[index].polling_flag--;
					
					if(m_claw[index].status == STDL_CLAW_INITING
						&& (p_recv->data & CLAW_STATUS_INIT_DONE)
					  && (p_recv->data & CLAW_STATUS_RUN_STOP) )
					{
						m_claw[index].status = STDL_CLAW_CLOSE;
					}
					else if(m_claw[index].status == STDL_CLAW_OPENNING
						&& (p_recv->data & CLAW_STATUS_RUN_STOP)
						&& ((p_recv->data & CLAW_STATUS_CLOSED) == 0) )
					{
						if(p_recv->data & CLAW_STATUS_STALLED)
						{
							Critical_Log("Claw index %d open with clamp err!\r\n",index);
						}
						m_claw[index].status = STDL_CLAW_OPEN;
					}
					else if(m_claw[index].status == STDL_CLAW_CLOSING
						&& (p_recv->data & CLAW_STATUS_RUN_STOP)
						&& (p_recv->data & CLAW_STATUS_CLOSED) )
					{
						if(p_recv->data & CLAW_STATUS_STALLED)
							m_claw[index].status = STDL_CLAW_CLOSE;
						else
							m_claw[index].status = STDL_CLAW_CLAMP;
					}
					break;
				}
			}
		}
		else if(p_recv->sdo_cs == SDO_CS_READ_ACK && p_recv->regaddr == CLAW_REG_ERR)
		{
			Trace_Log("Can id 0x%03X read errcode 0x%04X",can_id,p_recv->data);
		}
		else if(p_recv->sdo_cs == SDO_CS_READ_ACK && p_recv->regaddr == CLAW_REG_VERSION)
		{
			Trace_Log("Can id 0x%03X read version 0x%04X",can_id,p_recv->data);
		}
	}
	rt_hw_interrupt_enable(it);
}

stdl_claw_status_t get_stdl_claw_status(claw_index_t index)
{
	return m_claw[index].status;
}